// Prototype
// Version 0.4.1

#include <p32xxxx.h>
#include <plib.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#include "HelmetSystemConfig.h"
#include "HelmetADC.h"
#include "HelmetTimers.h"
#include "HelmetLEDs.h"
#include "HelmetInputs.h"
#include "HelmetUART.h"
#include "HelmetI2C.h"
#include "HelmetOutputs.h"

/**************************************************************************************
	Main
**************************************************************************************/
main() {
	
	/**************************************
		Config System
	**************************************/
	ConfigSystem();
	
	/**************************************
		Configure Timer 1
	**************************************/
	ConfigTimer1();
    
    /**************************************
		Configure Atom
	**************************************/	
	ConfigAtom();
	
	/**************************************
		Configure GPS
	**************************************/	
    ConfigGPS();
    
    /**************************************
		Configure Fuel Gauge
	**************************************/
	ConfigFuelGauge();
    
    /**************************************
		Configure ADC
	**************************************/	
	ConfigADC();
	
	/**************************************
		Configure Inputs
	**************************************/
	ConfigInputs();

	/**************************************
		Configure LEDs
	**************************************/
	ConfigLEDs();
	
	/**************************************
		Configure Outputs
	**************************************/
	ConfigAllOutputs();
	
	/**************************************
		Enable interrupts
	**************************************/
    EnableInterrupts();
	
	/**************************************
		Startup Message
	**************************************/
	WriteAtom("====================================\r\n");
	
	/**************************************
		Main Loop
	**************************************/
	float actx, acty, actz; // actual x, y, z
	float gmag; // magnitude of g-force experienced
	char charx[10], chary[10], charz[101]; // string versions of x, y, z
	char chargmag[10]; // string version of gmag;
	float actht; // actual hangtime float
	char charht[10]; // hang time to output
	
	int i=0, j=1000000;
	char chari[10];
	
	while( 1 ) {		
		
		// Interpret ADC sample data
		if( ADCDone == 1 ) {			
			
			// See *CALCULATION NOTE* in HelmetADC.h
			// For description of calculations below
			actx = ( rawx - 512.225 ) / 63.95;
			acty = ( rawy - 524.15 ) / 64.25;
			actz = ( rawz - 495.45 ) / 62.7;
			gmag = sqrt( actx*actx + acty*acty + actz*actz ); // magnitude of g's experienced
			sprintf( chargmag, "%3.1f", gmag );
			
			/*
			sprintf( charx, "%4.1f", actx );
			sprintf( chary, "%4.1f", acty );
			sprintf( charz, "%4.1f", actz );
			WriteAtom( "X, Y, Z = " );
			WriteAtom( charx ); WriteAtom( ", " ); WriteAtom( chary ); WriteAtom( ", " ); WriteAtom( charz );
			WriteAtom("\r\n");
			*/
			
			WriteAtom( "G-force = " ); WriteAtom( chargmag ); WriteAtom( "\r\n" );
			
			ADCDone=0;
		}
		
		if( MainReportHangTime ) {
			actht = ( LogHangTime*1.0 - 1 ) / T1_FREQ ; // hangtime = logged hangtime / frequency of T1 interrupt
			sprintf( charht, "%.2f", actht ); 
			WriteAtom( "Hang Time: " );
			WriteAtom( charht );
			WriteAtom( "\r\n" );
			LogHangTime=0;
			MainReportHangTime=0;
		}
		
		if( MainCheckFuelGauge >= FUEL_GAUGE_CHECK_INTERVAL ) {
			// write divider
			WriteAtom( "-------------------------------\r\n" );
			
			// get fuel gauge status and write
			int fgstatus; 
			char fgstatusmsg[50];
			
			fgstatus = ReadFuelGaugeStatusReg();
			sprintf( fgstatusmsg, "Statup Fuel Gauge Status: %d\r\n", fgstatus );
			WriteAtom( fgstatusmsg );
			
			if( (fgstatus&0x06) > 0 ) {
				WriteAtom("Clearing Status Reg\r\n");
				ClearFuelGaugeStatusReg();
			}	
			
			float vin = GetFuelGaugeVinValue();
			sprintf( fgstatusmsg, "Voltage Measurement: %7.4f\r\n", vin );
			WriteAtom( fgstatusmsg );
			
			fgstatus = ReadFuelGaugeRARCReg();
			sprintf( fgstatusmsg, "RARC: %d%%\r\n", fgstatus );
			WriteAtom( fgstatusmsg );
			
			fgstatus = ReadFuelGaugeByte(0x07);
			sprintf( fgstatusmsg, "RSRC: %d%%\r\n", fgstatus );
			WriteAtom( fgstatusmsg );
			
			if( fgstatus == 0 ) {
				WriteAtom("Setting batt full\r\n");
				SetBattLevelToFull();
			}
			
			fgstatus = ReadFuelGaugeTwoBytes(0x02); // RAAC
			vin = fgstatus*1.6;
			sprintf( fgstatusmsg, "RAAC: %.1f mAh\r\n", vin );
			WriteAtom( fgstatusmsg );
			
			fgstatus = ReadFuelGaugeTwoBytes(FUEL_GAUGE_ACR_REG);
			sprintf( fgstatusmsg, "ACR: %d\r\n", fgstatus );
			WriteAtom( fgstatusmsg );
			
			fgstatus = ReadFuelGaugeTwoBytes(0x18); // active empty
			sprintf( fgstatusmsg, "AE: %d\r\n", fgstatus );
			WriteAtom( fgstatusmsg );
			
			fgstatus = ReadFuelGaugeTwoBytes(0x16); // Full
			sprintf( fgstatusmsg, "FULL: %d\r\n", fgstatus );
			WriteAtom( fgstatusmsg );
			
			fgstatus = ReadFuelGaugeTwoBytes(0x6A); // Full40
			sprintf( fgstatusmsg, "FULL40: %d\r\n", fgstatus );
			WriteAtom( fgstatusmsg );
			
			fgstatus = ReadFuelGaugeTwoBytes(0x0A); // temp
			vin = ((short int)fgstatus)/32*0.125; // .125 *C resolution
			sprintf( fgstatusmsg, "Temp: %.4f *C\r\n", vin );
			WriteAtom( fgstatusmsg );
			
			fgstatus = ReadFuelGaugeTwoBytes(0x0E); // current
			vin = ((short int)fgstatus)*0.0003125; // 312.5 uA resolution
			sprintf( fgstatusmsg, "Current: %.4f A\r\n", vin );
			WriteAtom( fgstatusmsg );
			
			fgstatus = ReadFuelGaugeByte(FUEL_GAUGE_RSNSP_RAM);
			sprintf( fgstatusmsg, "RSNSP: %d S\r\n", fgstatus );
			WriteAtom( fgstatusmsg );	
			
			fgstatus = ReadFuelGaugeByte(FUEL_GAUGE_VCHG_RAM);
			sprintf( fgstatusmsg, "VCHG: %d\r\n", fgstatus );
			WriteAtom( fgstatusmsg );
			
			fgstatus = ReadFuelGaugeByte(FUEL_GAUGE_VAE_RAM);
			sprintf( fgstatusmsg, "VAE: %d\r\n", fgstatus );
			WriteAtom( fgstatusmsg );
			
			fgstatus = ReadFuelGaugeByte(FUEL_GAUGE_IMIN_RAM);
			sprintf( fgstatusmsg, "IMIN: %d\r\n", fgstatus );
			WriteAtom( fgstatusmsg );
			
			fgstatus = ReadFuelGaugeByte(FUEL_GAUGE_IAE_RAM);
			sprintf( fgstatusmsg, "IAE: %d\r\n", fgstatus );
			WriteAtom( fgstatusmsg );
			
			fgstatus = ReadFuelGaugeByte(FUEL_GAUGE_AC_MSB_RAM) << 8;
			fgstatus += ReadFuelGaugeByte(FUEL_GAUGE_AC_LSB_RAM);
			sprintf( fgstatusmsg, "AC: %d\r\n", fgstatus );
			WriteAtom( fgstatusmsg );
			
			fgstatus = ReadFuelGaugeByte(FUEL_GAUGE_AS_REG);
			sprintf( fgstatusmsg, "AS: %d\r\n", fgstatus );
			WriteAtom( fgstatusmsg );
			
			// write divider
			WriteAtom( "-------------------------------\r\n" );
			
			MainCheckFuelGauge=0;
		}		
		
	}
}

